package ar.edu.untref.robotica.simulador;

import java.awt.Image;
import java.awt.Point;
import java.io.File;
import java.io.IOException;
import java.math.BigDecimal;
import java.util.LinkedList;
import java.util.List;
import java.util.Timer;
import java.util.TimerTask;

import javax.imageio.ImageIO;

import ar.edu.untref.robotica.control.ConexionRemota;
import ar.edu.untref.robotica.control.ModuloDeControl;
import ar.edu.untref.robotica.control.Teclado;
import ar.edu.untref.robotica.log.Log;
import ar.edu.untref.robotica.motorfisico.Calculadora;

public class Simulador extends TimerTask {

	private int fps = 120;
	private Graficador graficador;
	private Escenario escenario;
	private Robot robot;
	private double velocidadFinalDeseadaIzquierda = 0;
	private double velocidadFinalDeseadaDerecha = 0;
	private double fuerzaAceleracion = 0.05;
	private Posicion posicionAnterior;
	@SuppressWarnings("unused")
	private ModuloDeControl teclado;
	@SuppressWarnings("unused")
	private ModuloDeControl controlador;
	private Log logger;
	private int contadorLog = 0;
	private List<String[]> listaDeDatos;

	public Simulador(Escenario escenario) {

		this.escenario = escenario;
		this.listaDeDatos = new LinkedList<String[]>();
	}

	public void inicializar() {

		this.graficador = new Graficador(this.escenario);

		this.robot = new Robot(Configuracion.instancia().getAltoRobot(),
				Configuracion.instancia().getAnchoRobot());
		Timer timer = new Timer();
		double period = (double) 1 / this.fps * 1000;
		timer.scheduleAtFixedRate(this, 0, (long) period);

		this.logger = new Log();

		this.teclado = new Teclado(this);
		this.controlador = new ConexionRemota(this);
	}

	@Override
	public void run() {

		if (this.graficador.modoSimulacion()) {

			/*
			 * Cambio dinámico de escenario
			 */
			if (this.graficador.cambioMapa()) {

				try {

					Image imagen = ImageIO.read(new File(this.graficador
							.getNuevoMapa()));
					this.escenario.setImagen(imagen);

				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
			}

			/*
			 * Cambio de la ruta del logger
			 */
			if (this.graficador.cambioLog()) {
				try {
					this.logger.crearFile();
				} catch (IOException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
				this.contadorLog = 0;
			}

			if (Calculadora.getVelocidadDesplazamiento(
					this.velocidadFinalDeseadaIzquierda,
					this.velocidadFinalDeseadaDerecha) > 0) {
				if (!puedeAvanzar()) {
					acelerarRobot(0, 0);
				}
			} else if (Calculadora.getVelocidadDesplazamiento(
					this.velocidadFinalDeseadaIzquierda,
					this.velocidadFinalDeseadaDerecha) < 0) {
				if (!puedeRetroceder()) {
					acelerarRobot(0, 0);
				}
			}
			this.logicaAceleracion();
			this.escenario.aumentarAngulo(Calculadora.getCambioAngulo(
					this.robot.getRuedaI(), this.robot.getRuedaD()) / this.fps);
			this.reflejarMovimiento();
			this.graficador.sensar(this.robot);

			if (this.graficador.isLogEnabled()) {
				this.contadorLog++;
				this.listaDeDatos.add(graficador.datosRelevantesAGuardar());
				if (contadorLog == this.fps) {
					try {
						this.logger.guardarDatos(listaDeDatos);
						listaDeDatos.clear();
						this.contadorLog = 0;
					} catch (IOException e) {
						e.printStackTrace();
					}
				}
			}

			if (this.hayChoque()) {
				forzarFrenado();
				this.escenario.setPosicionRobot(posicionAnterior);
				this.graficador.choque(true);
			} else {
				this.posicionAnterior = this.escenario.getPosicionRobot()
						.clone();
				this.graficador.choque(false);
			}
		} else {

			forzarFrenado();
			this.graficador.sensar(this.robot);

		}
		this.graficador.repaint();
	}

	private void forzarFrenado() {

		robot.getRuedaD().setVelocidad(new BigDecimal(0));
		robot.getRuedaI().setVelocidad(new BigDecimal(0));
		this.acelerarRobot(0, 0);
	}

	public boolean puedeAvanzar() {

		double distanciaSegura = Configuracion.instancia().getDistanciaSegura();
		return this.robot.getDistanciaObstaculoAdelante(this.escenario) > distanciaSegura
				&& this.robot.getDistanciaObstaculo45Derecha(this.escenario) > distanciaSegura
				&& this.robot.getDistanciaObstaculo45Izquierda(this.escenario) > distanciaSegura;
	}

	public boolean puedeRetroceder() {

		double distanciaSegura = Configuracion.instancia().getDistanciaSegura();
		return this.robot.getDistanciaObstaculoAtras(this.escenario) > distanciaSegura;
	}

	public void reflejarMovimiento() {

		Posicion posicionRobot = this.escenario.getPosicionRobot();
		double posicionXActual = posicionRobot.getPosicionX();
		double posicionYActual = posicionRobot.getPosicionY();
		double velocidad = this.robot.getVelocidadDesplazamiento();
		double angulo = Math.toRadians(this.escenario.getPosicionRobot()
				.getAngulo());
		double posicionXNueva = posicionXActual + velocidad * Math.sin(angulo);
		double posicionYNueva = posicionYActual - velocidad * Math.cos(angulo);
		posicionRobot.setPosicionX(posicionXNueva);
		posicionRobot.setPosicionY(posicionYNueva);
	}

	private void logicaAceleracion() {

		BigDecimal deltaDeAceleracion = new BigDecimal(this.fuerzaAceleracion);
		if (!velocidadDeseadaAlcanzadaIzquierda()) {
			if (this.velocidadFinalDeseadaIzquierda > this.robot.getRuedaI()
					.getVelocidad().doubleValue()) {
				this.robot.getRuedaI().setVelocidad(
						this.robot.getRuedaI().getVelocidad()
								.add(deltaDeAceleracion));
			} else if (this.velocidadFinalDeseadaIzquierda < this.robot
					.getRuedaI().getVelocidad().doubleValue()) {
				this.robot.getRuedaI().setVelocidad(
						this.robot.getRuedaI().getVelocidad()
								.subtract(deltaDeAceleracion));
			}
		}
		if (!velocidadDeseadaAlcanzadaDerecha()) {
			if (this.velocidadFinalDeseadaDerecha > this.robot.getRuedaD()
					.getVelocidad().doubleValue()) {
				this.robot.getRuedaD().setVelocidad(
						this.robot.getRuedaD().getVelocidad()
								.add(deltaDeAceleracion));
			} else if (this.velocidadFinalDeseadaDerecha < this.robot
					.getRuedaD().getVelocidad().doubleValue()) {
				this.robot.getRuedaD().setVelocidad(
						this.robot.getRuedaD().getVelocidad()
								.subtract(deltaDeAceleracion));
			}
		}
	}

	public void acelerarRobot(double velocidadFinalIzquierda,
			double velocidadFinalDerecha) {

		this.velocidadFinalDeseadaIzquierda = velocidadFinalIzquierda;
		this.velocidadFinalDeseadaDerecha = velocidadFinalDerecha;
	}

	private boolean velocidadDeseadaAlcanzadaIzquierda() {

		return this.robot.getRuedaI().getVelocidad().doubleValue() == this.velocidadFinalDeseadaIzquierda;
	}

	private boolean velocidadDeseadaAlcanzadaDerecha() {

		return this.robot.getRuedaD().getVelocidad().doubleValue() == this.velocidadFinalDeseadaDerecha;
	}

	private boolean hayChoque() {

		boolean choco = false;
		int x = this.escenario.getShapeRobot().getBounds().x;
		int y = this.escenario.getShapeRobot().getBounds().y;
		int ancho = this.escenario.getShapeRobot().getBounds().width;
		int alto = this.escenario.getShapeRobot().getBounds().height;
		for (int i = x; i < x + ancho && i < this.escenario.getMatriz().length
				&& !choco; i++) {
			for (int j = y; j < y + alto
					&& j < this.escenario.getMatriz()[i].length && !choco; j++) {
				if (this.escenario.getMatriz()[i][j] == 1) {
					Point cadaPunto = new Point(i, j);
					if (this.escenario.getShapeRobot().contains(cadaPunto)) {
						choco = true;
					}
				}
			}
		}
		return choco;
	}

	public double getVelocidadFinalDeseadaIzquierda() {

		return velocidadFinalDeseadaIzquierda;
	}

	public double getVelocidadFinalDeseadaDerecha() {

		return velocidadFinalDeseadaDerecha;
	}

	public Escenario getEscenario() {
		return escenario;
	}

	public Robot getRobot() {
		return robot;
	}

	public Graficador getGraficador() {
		return graficador;
	}

}
